{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2 as cv\n",
    "import numpy as np\n",
    "import apriltag\n",
    "from math import *\n",
    "from uarm.wrapper import SwiftAPI\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "import apriltag\n",
    "\n",
    "# 定义一个检测器（使用字典“tag36h10”）\n",
    "at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11 tag36h10'))\n",
    "\n",
    "mtx = np.load('./data/camera_param.npz')['mtx']\n",
    "dist = np.load('./data/camera_param.npz')['dist']\n",
    "\n",
    "swift = SwiftAPI()\n",
    "swift.reset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'OK'"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "swift.set_servo_detach()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'OK'"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "swift.set_servo_attach()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[199.5179, 0.6121, 149.0863]"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "swift.get_position()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "等待\n",
      "-1\n",
      "等待\n",
      "-1\n",
      "等待\n",
      "49\n",
      "------------------\n",
      "[269.55460849 -16.88011987  10.16337835]\n",
      "等待\n",
      "27\n",
      "等待\n",
      "27\n"
     ]
    }
   ],
   "source": [
    "cv.namedWindow(\"image\")\n",
    "\n",
    "\n",
    "def waitMouse(event,x,y,flags,param):\n",
    "    if event == cv.EVENT_LBUTTONDOWN:\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "cap = cv.VideoCapture(4)\n",
    "swift.reset()\n",
    "cv.setMouseCallback(\"image\",waitMouse)\n",
    "_,frame = cap.read()\n",
    "while cap.isOpened():\n",
    "    _,frame = cap.read()\n",
    "    cv.imshow(\"image\",frame)\n",
    "    key = cv.waitKey(10)\n",
    "    if key == 27:\n",
    "        break\n",
    "        \n",
    "        \n",
    "cap.release()\n",
    "cv.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "with np.load('./data/eyehand_Matrix.npz') as X:\n",
    "    R, t = [X[i] for i in ('R', 't')]\n",
    "\n",
    "def eulerAngles2RotationMatrix(theta):\n",
    "    \"\"\"\n",
    "    欧拉角转换旋转矩阵\n",
    "    :param theta: 欧拉角角度\n",
    "    :return: 旋转矩阵\n",
    "    \"\"\"\n",
    "    R_x = np.array([[1, 0, 0],\n",
    "                    [0, cos(theta[0]), sin(theta[0])],\n",
    "                    [0, -sin(theta[0]), cos(theta[0])]\n",
    "                    ])\n",
    "    R_y = np.array([[cos(theta[1]), 0, sin(theta[1])],\n",
    "                    [0, 1, 0],\n",
    "                    [-sin(theta[1]), 0, cos(theta[1])]\n",
    "                    ])\n",
    "    R_z = np.array([[cos(theta[2]), -sin(theta[2]),0],\n",
    "                    [sin(theta[2]), cos(theta[2]),0],\n",
    "                    [0, 0, 1]\n",
    "                    ])\n",
    "    R = R_z@R_y@R_x\n",
    "    return R\n",
    "\n",
    "def get_base2end():\n",
    "    angle = swift.get_polar()[1]\n",
    "    position = swift.get_position()\n",
    "    R = eulerAngles2RotationMatrix([0,0,(angle-90)*pi/180])\n",
    "    t = position\n",
    "    return R,t\n",
    "\n",
    "def goto_imgpoint(u,v,h):\n",
    "    base2end_R,base2end_t = get_base2end()\n",
    "    depth = base2end_t-R@t\n",
    "    img2obj = np.linalg.inv(mtx)@np.array([u,v,1])*(depth[2]-h)\n",
    "    base2end_R, base2end_t = get_base2end()\n",
    "    dst = base2end_t + base2end_R @ R @ img2obj + base2end_R @ -R@t\n",
    "    swift.set_position(dst[0],dst[1],int(dst[2]))\n",
    "\n",
    "def get_img2tag(image,tag_size):\n",
    "    img2tag_p = []\n",
    "    cam_params = [mtx[0, 0], mtx[1, 1], mtx[0, 2], mtx[1, 2]]\n",
    "    gray = cv.cvtColor(image,cv.COLOR_BGR2GRAY)\n",
    "    tags = at_detector.detect(gray)\n",
    "    for tag in tags:\n",
    "        M, e1, e2 = at_detector.detection_pose(tag, cam_params, tag_size)\n",
    "        img2tag_p.append([tag.tag_id,tag.center,M])\n",
    "    return img2tag_p\n",
    "\n",
    "def get_base2tag_p(tags,id,R,t):\n",
    "    ret = False\n",
    "    dst = []\n",
    "    for tag in tags:\n",
    "        if tag[0]==id:\n",
    "            pix2img_t = np.linalg.inv(mtx) @ np.insert(tag[1], 2, 1)\n",
    "            img2tag_t = (tag[2] @ np.insert(pix2img_t, 3, 1))[:3]\n",
    "            base2end_R, base2end_t = get_base2end()\n",
    "            dst = base2end_t + base2end_R @ R @ img2tag_t + base2end_R @ -R@t\n",
    "            ret = True\n",
    "            print(dst)\n",
    "    return ret,dst\n",
    "\n",
    "def goto_mark(image,id):\n",
    "    tags = get_img2tag(image, 20)\n",
    "    ret, dst_p = get_base2tag_p(tags, id, R, t)\n",
    "    if ret == True:\n",
    "        swift.set_position(dst_p[0], dst_p[1], int(dst_p[2]))\n",
    "    else:\n",
    "        print(\"没有识别到id%d\",id)\n",
    "    return ret"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "swift.set_position(236,19,10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[199.6918, 0.0, 150.292]"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "swift.get_position()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "swift.reset()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([186.75362052,  79.89682824, 174.79925228])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "base2end_R,base2end_t = get_base2end()\n",
    "base2end_t-R@t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'OK'"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "swift.set_pump(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 19.54875229, -36.93952696,  25.1215327 ])"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1.80340183e+02, 2.54365797e-05, 1.49664400e+02])"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "base2end_t@base2end_R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "base2tag_p = np.load(\"./data/base2mark_pose.npz\")['pose']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 2.014392e+02,  1.144540e+01,  2.895000e-01],\n",
       "       [ 2.003990e+02, -1.818680e+01,  6.559000e-01],\n",
       "       [ 1.712183e+02,  1.157400e+01, -3.537000e-01],\n",
       "       [ 1.706427e+02, -1.865900e+01, -1.976000e-01]])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "base2tag_p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[200.,  15.,   0.],\n",
       "       [200., -15.,   0.],\n",
       "       [170.,  15.,   0.],\n",
       "       [170., -15.,   0.]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.array([[200.,15.,0.],\n",
    "          [200.,-15.,0.],\n",
    "          [170.,15,0.],\n",
    "          [170.,-15,0.]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
